/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-12-12     xu       the first version
 */
#include "W25Qx.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <rtthread.h>

#define printf(fmt, ...) rt_kprintf(fmt, ##__VA_ARGS__)

uint8_t wData[0x100];
uint8_t rData[0x100];

static int w25qx_test(int argc, char* argv[])
{
    uint8_t ID[4] = {0};
    uint32_t i = 0;
    printf("SPI-W25Qxxx Test start...\n");

    /*##-1- Read the device ID  ########################*/
    BSP_W25Qx_Init();
    BSP_W25Qx_Read_ID(ID);
    printf("W25Qxxx ID is : ");
    for (i = 0; i < 2; i++)
    {
        printf("0x%X ", ID[i]);
    }
    printf("\n");

    /*##-2- Erase Block ##################################*/
    if (BSP_W25Qx_Erase_Block(0) == W25Qx_OK)
    {
        printf("Erase Block OK\n");
    }
    else
    {
        printf("Erase Block FAIL!\n");
    }

    /*##-2- Written to the flash ########################*/
    /* fill buffer */
    for (i = 0; i < 0x100; i++)
    {
        wData[i] = i;
        rData[i] = 0;
    }

    if (BSP_W25Qx_Write(wData, 0x00, 0x100) == W25Qx_OK)
    {
        printf("Write OK\n");
    }
    else
    {
        printf("Write FAIL\n");
    }

    /*##-3- Read the flash     ########################*/
    if (BSP_W25Qx_Read(rData, 0x00, 0x100) == W25Qx_OK)
    {
        printf("Read OK\n");
    }
    else
    {
        printf("Read FAIL\n");
    }

    printf("Read Data :\n");
    for (i = 0; i < 0x100; i++)
        printf("0x%X  ", rData[i]);
    printf("\n");

    /*##-4- check data          ########################*/
    if (memcmp(wData, rData, 0x100) == 0)
    {
        printf(" W25Q128FV QuadSPI Test OK\n");
    }
    else
    {
        printf(" W25Q128FV QuadSPI Test FAIL\n");
    }

    return 0;
}
MSH_CMD_EXPORT(w25qx_test, W25Q128 test);

static void print_flash_data(uint32_t offset, const char *buf, rt_size_t size)
{
#define WIDTH_SIZE           32
    rt_size_t i, j;

    for (i = 0; i < size; i += WIDTH_SIZE) {
        rt_kprintf("%04X-%04X: ", offset + i, offset + i + WIDTH_SIZE);
        for (j = 0; j < WIDTH_SIZE; j++) {
            if (i + j < size) {
                rt_kprintf("%02X ", buf[i + j]);
            } else {
                rt_kprintf("   ");
            }
            if ((j + 1) % 8 == 0) {
                rt_kprintf(" ");
            }
        }
        rt_kprintf("\n");
    }
}

#define CMD_W25Q_USAGE() \
do { \
    rt_kprintf("Usage: %s -e addr sectors|-r addr size|-w addr size [dummy]\n", __func__); \
    rt_kprintf("    -e addr sectors         Erase sectors of flash area.\n"); \
    rt_kprintf("    -f                      Format all sectors on flash.\n"); \
    rt_kprintf("    -r addr size            Read a region of flash area.\n"); \
    rt_kprintf("    -w addr size [dummy]    Write a region of flash area.\n"); \
    rt_kprintf("    -i                      Initialize W25Q128 device.\n"); \
} while(0)

static uint8_t write_sector(uint32_t addr, void* buffer, uint32_t size)
{
    uint8_t status;
    uint32_t i;

    rt_kprintf("Write sector 0x%X ...\n", addr);
    RT_ASSERT((addr & (W25Q128FV_SECTOR_SIZE - 1)) == 0);
    RT_ASSERT((size & (W25Q128FV_SECTOR_SIZE - 1)) == 0 && size == W25Q128FV_SECTOR_SIZE);

    for (i = 0; i < W25Q128FV_SECTOR_SIZE/W25Q128FV_PAGE_SIZE; i++) {
        rt_kprintf("Write page 0x%X ...\n", addr);
        status = BSP_W25Qx_Write(buffer, addr, W25Q128FV_PAGE_SIZE);
        if (status != W25Qx_OK) {
            rt_kprintf("Write page 0x%X FAIL!\n", addr);
            return status;
        }
        addr += W25Q128FV_PAGE_SIZE;
    }
    return W25Qx_OK;
}

static uint8_t dummy = 0;
static uint8_t increment = 0;
uint8_t data()
{
    if (increment) {
        return dummy += increment;
    }
    return dummy;
}

static int w25q(int argc, char* argv[])
{
    long addr, size;
    long remain, blksz, i;
    uint8_t status = 0;
    uint32_t start_addr, end_addr;
    uint32_t first_sector, last_sector;
    uint8_t* buffer = 0;

    char opcode;
    const char* op_arg;

    if (argc <= 1) {
        CMD_W25Q_USAGE();
        return -1;
    }

    op_arg = argv[1];
    if (strlen(op_arg) != 2 || op_arg[0] != '-') {
        CMD_W25Q_USAGE();
        return -1;
    }

    switch (opcode) {
    case 'e':
    case 'r':
    case 'w':
        if (argc <= 4) {
            CMD_W25Q_USAGE();
            return -1;
        }
        break;
    }

    opcode = op_arg[1];

    if (argc >= 4) {
        addr = strtol(argv[2], NULL, 10);
        size = strtol(argv[3], NULL, 10);
        dummy = argc > 4 ? strtol(argv[4], NULL, 16) : 0x0;
        increment = argc > 5 ? atoi(argv[5]) : 0;

        start_addr = addr;
        end_addr = addr + size;
        rt_kprintf("--- --- --- --- --- --- --- --- \n");
        rt_kprintf("      size = %d\n", size);
        rt_kprintf("     dummy = %d\n", dummy);
        rt_kprintf("start_addr = 0x%X\n", addr);
        rt_kprintf("  end_addr = 0x%X\n", end_addr);

        first_sector = start_addr - (start_addr & (W25Q128FV_SECTOR_SIZE - 1));
        if (start_addr != first_sector) {
            rt_kprintf("start_addr 0x%X not sector aligned!\n");
        }

        last_sector = end_addr - (end_addr & (W25Q128FV_SECTOR_SIZE - 1));
        if (end_addr != last_sector) {
            rt_kprintf("end_addr 0x%X not sector aligned!\n");
        } else {
            last_sector -= W25Q128FV_SECTOR_SIZE;
        }
        rt_kprintf("first_sector = 0x%X\n", first_sector);
        rt_kprintf(" last_sector = 0x%X\n", last_sector);
        rt_kprintf("--- --- --- --- --- --- --- --- \n");
    }

    remain = size;
    addr = start_addr;
    buffer = rt_malloc(W25Q128FV_SECTOR_SIZE);

    switch (opcode) {
    case 'i':
        status = BSP_W25Qx_Init();
        if (status != W25Qx_OK) {
            rt_kprintf("Init W25Q FAIL!\n");
        } else {
            rt_kprintf("Init W25Q OK!\n");
        }
        break;
    case 'e':
        if (size >= W25Q128FV_SECTOR_NUMS) {
            rt_kprintf("WARNING: erasing all sectors...\n", size);
            status = BSP_W25Qx_Erase_Chip();
            if (status != W25Qx_OK) {
                rt_kprintf("Erase chip FAIL!\n", addr);
            } else {
                rt_kprintf("Erase chip OK!\n", addr);
            }
        } else {
            rt_kprintf("WARNING: erasing %d sectors...\n", size);
            addr = first_sector;
            for (i = 0; i < size; i++) {
                rt_kprintf("[%d/%d]Erasing sector at 0x%X...\n", i, size, addr);
                status = BSP_W25Qx_Erase_Block(addr);
                if (status != W25Qx_OK) {
                    rt_kprintf("Erase 0x%X FAIL!\n", addr);
                } else {
                    rt_kprintf("Erase 0x%X OK!\n", addr);
                }
                addr += W25Q128FV_SECTOR_SIZE;
            }
        }
        break;
    case 'f':
        rt_kprintf("WARNING: erasing all sectors...\n", size);
        status = BSP_W25Qx_Erase_Chip();
        if (status != W25Qx_OK) {
            rt_kprintf("Erase chip FAIL!\n", addr);
        } else {
            rt_kprintf("Erase chip OK!\n", addr);
        }
        break;
    case 'r':
        addr = start_addr; // - (start_addr & (W25Q128FV_PAGE_SIZE - 1));
        while (addr < end_addr) {
            blksz = (end_addr - addr) < W25Q128FV_PAGE_SIZE ? (end_addr - addr) : W25Q128FV_PAGE_SIZE;

            rt_kprintf("Read addr = 0x%X, blksz = %d ...\n", addr, blksz);
            rt_memset(buffer, 0xFF, blksz);
            status = BSP_W25Qx_Read(buffer, addr, blksz);
            if (status != W25Qx_OK) {
                rt_kprintf("Read page 0x%X FAIL!\n", addr);
            }

            print_flash_data(addr, buffer, blksz);
            addr += blksz;
        }
        break;
    case 'w':
        addr = start_addr;
        while (addr < end_addr) {
            blksz = (end_addr - addr) < W25Q128FV_PAGE_SIZE ? (end_addr - addr) : W25Q128FV_PAGE_SIZE;
            for (i = 0; i < blksz; i++) {
                buffer[i] = dummy++;
            }

            rt_kprintf("Write addr = 0x%X, blksz = %d ...\n", addr, blksz);
            status = BSP_W25Qx_Write(buffer, addr, blksz);
            if (status != W25Qx_OK) {
                rt_kprintf("Write page 0x%X FAIL!\n", addr);
            }
            print_flash_data(addr, buffer, blksz);
            addr += blksz;
        }
        break;
    }
    rt_free(buffer);
}
MSH_CMD_EXPORT(w25q, W25Qx test);

#if 0
#define CMD_W25Q_OVERWRITE_USAGE() \
do { \
    rt_kprintf("Usage: %s -W addr size [dummy] [inc]\n", __func__); \
    rt_kprintf("    -W addr size [dummy]    Overwrite a region of flash area.\n", __func__); \
} while(0)

static int w25q_overwrite_test(int argc, char* argv[])
{
    long addr, size;
    long blksz, i;
    uint8_t status = 0;
    uint32_t start_addr, end_addr;
    uint32_t first_sector, last_sector;
    uint8_t* buffer = 0;


    char opcode;
    const char* op_arg;

    if (argc <= 1) {
        CMD_W25Q_OVERWRITE_USAGE();
        return -1;
    }

    op_arg = argv[1];
    if (strlen(op_arg) != 2 || op_arg[0] != '-') {
        CMD_W25Q_OVERWRITE_USAGE();
        return -1;
    }

    switch (opcode) {
    case 'W':
        if (argc <= 4) {
            CMD_W25Q_OVERWRITE_USAGE();
            return -1;
        }
        break;
    default:
        CMD_W25Q_OVERWRITE_USAGE();
        return -1;
        break;
    }

    opcode = op_arg[1];
    if (argc >= 4) {
        addr = strtol(argv[2], NULL, 10);
        size = strtol(argv[3], NULL, 10);
        dummy = argc > 4 ? strtol(argv[4], NULL, 16) : 0x0;
        increment = argc > 5 ? atoi(argv[5]) : 0;

        start_addr = addr;
        end_addr = addr + size;
        rt_kprintf("--- --- --- --- --- --- --- --- \n");
        rt_kprintf("      size = %d\n", size);
        rt_kprintf("     dummy = %d\n", dummy);
        rt_kprintf("start_addr = 0x%X\n", addr);
        rt_kprintf("  end_addr = 0x%X\n", end_addr);

        first_sector = start_addr - (start_addr & (W25Q128FV_SECTOR_SIZE - 1));
        if (start_addr != first_sector) {
            rt_kprintf("start_addr 0x%X not sector aligned!\n");
        }

        last_sector = end_addr - (end_addr & (W25Q128FV_SECTOR_SIZE - 1));
        if (end_addr != last_sector) {
            rt_kprintf("end_addr 0x%X not sector aligned!\n");
        } else {
            last_sector -= W25Q128FV_SECTOR_SIZE;
        }
        rt_kprintf("first_sector = 0x%X\n", first_sector);
        rt_kprintf(" last_sector = 0x%X\n", last_sector);
        rt_kprintf("--- --- --- --- --- --- --- --- \n");
    }

    addr = start_addr;
    buffer = rt_malloc(W25Q128FV_SECTOR_SIZE);

    switch (opcode) {
    case 'W':
        addr = first_sector;
        while (addr < end_addr) {
            uint8_t need_erase = 0;

            // read the sector data
            rt_memset(buffer, 0xFF, W25Q128FV_SECTOR_SIZE);
            status = BSP_W25Qx_Read(buffer, addr, W25Q128FV_SECTOR_SIZE);
            if (status != W25Qx_OK) {
                rt_kprintf("Read head 0x%X FAIL!\n", first_sector);
            }

            // check target area is writable or not,
            // and prepare new sector data in buffer
            if (addr == first_sector && start_addr != first_sector) {
                long prev_size = start_addr - first_sector; // previous area size
                long wend_size = (prev_size + size) < W25Q128FV_SECTOR_SIZE ? prev_size + size : W25Q128FV_SECTOR_SIZE;
                // try write in buffer
                for (i = prev_size; i < wend_size; i++) {
                    if (buffer[i] != 0xFF) {
                        need_erase = 1;
                    }
                    buffer[i] = data();
                }
            } else if (addr == last_sector && end_addr != last_sector) {
                long post_size = end_addr - last_sector; // post area size
                // try write in buffer
                for (i = 0; i < post_size; i++) {
                    if (buffer[i] != 0xFF) {
                        need_erase = 1;
                    }
                    buffer[i] = data();
                }
            } else {
                // try write in buffer
                for (i = 0; i < W25Q128FV_SECTOR_SIZE; i++) {
                    if (buffer[i] != 0xFF) {
                        need_erase = 1;
                    }
                    buffer[i] = data();
                }
            }

            // erase the sector when need
            if (need_erase) {
                status = BSP_W25Qx_Erase_Block(first_sector);
                if (status != W25Qx_OK) {
                    rt_kprintf("Erase sector 0x%X FAIL!\n", first_sector);
                }
            }

            // write buffer to sector
            status = write_sector(addr, buffer, W25Q128FV_SECTOR_SIZE);
            if (status != W25Qx_OK) {
                rt_kprintf("Write sector 0x%X FAIL!\n", addr);
            }

            addr += W25Q128FV_SECTOR_SIZE;
        }

        break;
    }
    rt_free(buffer);
}
MSH_CMD_EXPORT_ALIAS(w25q_overwrite_test, w25qot, W25Qx overwrite test);
#endif
